

#include "quadStability_TASK.h"

#include "FreeRTOS.h"
#include "queue.h"
#include "task.h"
#include "semphr.h"

#include "imuFilterComplementary.h"
#include "imuFilterQuaternion.h"

#include "fftc.h"
#include "math.h"

#include "state.h"
#include "config.h"
#include "driverRN41_UART_DMA_ISR_TASK.h"
#include "qmath.h"

#include "tools.h"

void stabAccGenerateMatrix();
void stabAccCalibrate();


enum {
    IMU_MODE_COMPLEMENTARY = 0,
    IMU_MODE_QUATERNION
} IMUMode;

const char * enumIMUMode[] = {
    [IMU_MODE_COMPLEMENTARY] = "Complementary",
    [IMU_MODE_QUATERNION] = "Quaternion",
    ""
};

void stabGravChanged(const void * ref, const void * values)
{
	CONFIG.mpu6050.accCalibration.x = INT_FROM_BYTES_REV(((BYTE*)values),0);
	CONFIG.mpu6050.accCalibration.y = INT_FROM_BYTES_REV(((BYTE*)values),4);
	CONFIG.mpu6050.accCalibration.z = INT_FROM_BYTES_REV(((BYTE*)values),8);

	stabAccGenerateMatrix();
}

void stabAccGenerateMatrix()
{
	if (CONFIG.mpu6050.accCalibration.x == 0 && CONFIG.mpu6050.accCalibration.y == 0 && CONFIG.mpu6050.accCalibration.z == 0)
		stabAccCalibrate();

	STATE.imu.accGravityNormalize = qDiv(Q_ONE, vectorLength_inline(CONFIG.mpu6050.accCalibration));

	q_vector_t inverseGravity;
	inverseGravity.x = -CONFIG.mpu6050.accCalibration.x;
	inverseGravity.y = -CONFIG.mpu6050.accCalibration.y;
	inverseGravity.z = -CONFIG.mpu6050.accCalibration.z;

	matrixAxis(&inverseGravity,&STATE.imu.accGravityMatrix);

	STATE.mpu6050.accMatrixSet = 1;
}


void stabAccCalibrate()
{
	CONFIG.mpu6050.accCalibration.x = CONFIG.mpu6050.accCalibration.y = CONFIG.mpu6050.accCalibration.z = 0;
	unsigned char i;
	for (i=0;i<MPU_ACC_BUFFERSIZE;i++)
	{
		CONFIG.mpu6050.accCalibration.x += (q_t)STATE.mpu6050.bufferAcc[i].x;
		CONFIG.mpu6050.accCalibration.y += (q_t)STATE.mpu6050.bufferAcc[i].y;
		CONFIG.mpu6050.accCalibration.z += (q_t)STATE.mpu6050.bufferAcc[i].z;
	}

	CONFIG.mpu6050.accCalibration.x /= MPU_ACC_BUFFERSIZE;
	CONFIG.mpu6050.accCalibration.y /= MPU_ACC_BUFFERSIZE;
	CONFIG.mpu6050.accCalibration.z /= MPU_ACC_BUFFERSIZE;

//	CONFIG.sensor.accGravity.x = q_mult(CONFIG.sensor.accGravity.x + CONFIG.sensor.offsetAcc.x, CONFIG.sensor.scaleAcc.x);
//	CONFIG.sensor.accGravity.y = q_mult(CONFIG.sensor.accGravity.y + CONFIG.sensor.offsetAcc.y, CONFIG.sensor.scaleAcc.y);
//	CONFIG.sensor.accGravity.z = q_mult(CONFIG.sensor.accGravity.z + CONFIG.sensor.offsetAcc.z, CONFIG.sensor.scaleAcc.z);

	stabAccGenerateMatrix();
}


void stabMagnetometerCalibrate()
{
	STATE.imu.magnBase.x = 0;
	STATE.imu.magnBase.y = 0;
	STATE.imu.magnBase.z = 0;
	unsigned char i;
	for (i=0;i<MAGN_BUFFERSIZE;i++)
	{
		STATE.imu.magnBase.x += (q_t)STATE.hmc5883.buffer[i].x;
		STATE.imu.magnBase.y += (q_t)STATE.hmc5883.buffer[i].y;
		STATE.imu.magnBase.z += (q_t)STATE.hmc5883.buffer[i].z;
	}

	STATE.imu.magnBase.x /= MAGN_BUFFERSIZE;
	STATE.imu.magnBase.y /= MAGN_BUFFERSIZE;
	STATE.imu.magnBase.z /= MAGN_BUFFERSIZE;

	STATE.imu.magnBaseLength = vectorLength_inline(STATE.imu.magnBase);

	STATE.hmc5883.calibrated = 1;
}

void stabProcessSensors()
{
	unsigned char i, p;
	q_vector_t sum;

	// GYROSCOPE
	{
		sum.x = sum.y = sum.z = 0;
		p = STATE.mpu6050.bufferGyroPosition;

		for (i=0;i<CONFIG.mpu6050.gyroSoftwareAvg[0];i++)
		{
			p--;
			if (p > MPU_GYRO_BUFFERSIZE) p = MPU_GYRO_BUFFERSIZE - 1;
			sum.x = qAdd(sum.x, STATE.mpu6050.bufferGyro[p].x);
			sum.y = qAdd(sum.y, STATE.mpu6050.bufferGyro[p].y);
		}
		p = STATE.mpu6050.bufferGyroPosition;
		for (i=0;i<CONFIG.mpu6050.gyroSoftwareAvg[1];i++)
		{
			p--;
			if (p > MPU_GYRO_BUFFERSIZE) p = MPU_GYRO_BUFFERSIZE - 1;
			sum.z = qAdd(sum.z, STATE.mpu6050.bufferGyro[p].z);
		}

		STATE.imu.gyroFiltered.x = qAdd(sum.x / CONFIG.mpu6050.gyroSoftwareAvg[0], -STATE.imu.gyroAutoBias.x);
		STATE.imu.gyroFiltered.y = qAdd(sum.y / CONFIG.mpu6050.gyroSoftwareAvg[0], -STATE.imu.gyroAutoBias.y);
		STATE.imu.gyroFiltered.z = qAdd(sum.z / CONFIG.mpu6050.gyroSoftwareAvg[1], -STATE.imu.gyroAutoBias.z);

		const int autoBiasSpeed = 1;
		if (STATE.imu.gyroFiltered.x != 0) STATE.imu.gyroAutoBias.x += STATE.imu.gyroFiltered.x > 0 ? autoBiasSpeed : -autoBiasSpeed;
		if (STATE.imu.gyroFiltered.y != 0) STATE.imu.gyroAutoBias.y += STATE.imu.gyroFiltered.y > 0 ? autoBiasSpeed : -autoBiasSpeed;
		if (STATE.imu.gyroFiltered.z != 0) STATE.imu.gyroAutoBias.z += STATE.imu.gyroFiltered.z > 0 ? autoBiasSpeed : -autoBiasSpeed;
        STATE.mpu6050.gyroBiassed = 1;
        
		STATE.imu.gyroScaledToSec = STATE.imu.gyroFiltered;

        SHORT scaleArray[] = {250,500,1000,2000};

        q_t gyroScale = Q_ONE * 2 * scaleArray[CONFIG.mpu6050.hardwareGyroRange] / CONFIG.imu.freq;
        vectorScale_inline(STATE.imu.gyroScaledToSec, gyroScale);
		
		//bluetoothLogData(0x0B,12,&STATE.imu.gyroScaledToSec);
        logStore(xTaskGetTickCount(), LOG_ID_GYRO_FILTERED, STATE.imu.gyroFiltered);
	}

	// ACCELEROMETER
	{
		sum.x = sum.y = sum.z = 0;

		// average filter
		p = STATE.mpu6050.bufferAccPosition;
		for (i=0;i<CONFIG.mpu6050.accSoftwareAvg;i++)
		{
			p--;
			if (p > MPU_ACC_BUFFERSIZE) p = MPU_ACC_BUFFERSIZE - 1;
			sum.x += STATE.mpu6050.bufferAcc[p].x;
			sum.y += STATE.mpu6050.bufferAcc[p].y;
			sum.z += STATE.mpu6050.bufferAcc[p].z;
		}

        //sum.x *= 32;
        //sum.y *= 32;
        //sum.z *= 32;

		STATE.imu.accFiltered.x = qMul((((q_t)sum.x / (q_t)CONFIG.mpu6050.accSoftwareAvg) + CONFIG.mpu6050.accOffset.x), CONFIG.mpu6050.accScale.x);
		STATE.imu.accFiltered.y = qMul((((q_t)sum.y / (q_t)CONFIG.mpu6050.accSoftwareAvg) + CONFIG.mpu6050.accOffset.y), CONFIG.mpu6050.accScale.y);
		STATE.imu.accFiltered.z = qMul((((q_t)sum.z / (q_t)CONFIG.mpu6050.accSoftwareAvg) + CONFIG.mpu6050.accOffset.z), CONFIG.mpu6050.accScale.z);

		// normalize to gravity
		STATE.imu.accFilteredScaled = STATE.imu.accFiltered;
		vectorScale_inline(STATE.imu.accFilteredScaled, STATE.imu.accGravityNormalize);

		// translate accelerometer reading
		matrixMultiplyVector(&STATE.imu.accGravityMatrix,&STATE.imu.accFilteredScaled,&STATE.imu.accFilteredTransposed);

		STATE.imu.accFilteredNormalised = STATE.imu.accFilteredTransposed;
		// unit vector
		vectorNormalize_inline(STATE.imu.accFilteredNormalised);

		//bluetoothLogData(0x0A,12,&STATE.imu.accFilteredTransposed);
        logStore(xTaskGetTickCount(), LOG_ID_ACC_FILTERED, STATE.imu.accFilteredTransposed);
	}

	// MAGNETOMETER
	{
		sum.x = sum.y = sum.z = 0;

		p = STATE.hmc5883.bufferPosition;

		for (i=0;i<CONFIG.hmc5883.softwareAvg;i++)
		{
			p--;
			if (p > MAGN_BUFFERSIZE) p = MAGN_BUFFERSIZE - 1;
			sum.x += STATE.hmc5883.buffer[p].x;
			sum.y += STATE.hmc5883.buffer[p].y;
			sum.z += STATE.hmc5883.buffer[p].z;
		}

        // results * 1024 => prevent vector length from being 0 (due to small values)

		STATE.imu.magnFiltered.x = (q_t)sum.x / (q_t)CONFIG.hmc5883.softwareAvg * 1024;
		STATE.imu.magnFiltered.y = (q_t)sum.y / (q_t)CONFIG.hmc5883.softwareAvg * 1024;
		STATE.imu.magnFiltered.z = (q_t)sum.z / (q_t)CONFIG.hmc5883.softwareAvg * 1024;
        logStore(xTaskGetTickCount(), LOG_ID_MAGN_FILTERED, STATE.imu.magnFiltered);
	}

	// BARO!
	{
		STATE.imu.baroFiltered = 0;
		p = STATE.mpl3115.bufferPosition;
		for (i=0;i<CONFIG.mpl3115.softwareAvg;i++)
		{
			p--;
			if (p > BARO_BUFFERSIZE) p = BARO_BUFFERSIZE - 1;
			STATE.imu.baroFiltered += (q_t)STATE.mpl3115.buffer[p];
		}
		STATE.imu.baroFiltered /= (q_t)CONFIG.mpl3115.softwareAvg;
		STATE.imu.baroFiltered -= STATE.imu.baroBase;
        logStore(xTaskGetTickCount(), LOG_ID_BARO_FILTERED, STATE.imu.baroFiltered);
	}
}

void TaskStability(void * params)
{
	TickType_t sleep;
	sleep = xTaskGetTickCount();

	int c;
	c = 0;
    int semFreq = 0;

    pidChanged(NULL,NULL);
    
	for(;;)
	{
        if (CONFIG.imu.useSemaphore == 1)
        {
            extern SemaphoreHandle_t semStability;
            if (xSemaphoreTake(semStability,portMAX_DELAY) != pdTRUE) continue;
        }
        else
        {
    		vTaskDelayUntil(&sleep,configTICK_RATE_HZ / CONFIG.imu.freq);
        }
        STATE.monitoring.stabilityTaskTickCount++;

		c++;

		if (!STATE.mpu6050.accBufferFilled ||
			!STATE.mpu6050.gyroBufferFilled)
		{
			//state.quadState.imuFix = 0;
			continue;
		}

		if (!STATE.mpu6050.accMatrixSet) stabAccGenerateMatrix();
		//if (!STATE.mpu6050.gyroBiassed) stabGyroBias();
		if (!STATE.hmc5883.calibrated) stabMagnetometerCalibrate();

		stabProcessSensors();

		if (CONFIG.imu.filter == 0)
			complementary();
		else
            quaternion();


        logStore(xTaskGetTickCount(), LOG_ID_IMU_HEADING, (STATE.imu.output.pitch));

        if (STATE.imu.mode > 0)
        {
            pidCalculateAll();
        }
            static int rr = 0;

            /*
            if (rr++ > 5)
            {
            btDebugInt(sleep);
            btDebugInt(STATE.imu.output.pitch);
            btDebugInt(STATE.imu.output.roll);
            btDebugInt(STATE.imu.output.yaw);
            rr = 0;
            }
             * */
		//bluetoothLogData(0x06,4,&STATE.imu.output.pitch);
		//bluetoothLogData(0x07,4,&STATE.imu.output.roll);

		/*{
			struct matrix4f imuAccRotation;
			float omega, theta, sintheta, costheta, sinomega, cosomega;
			omega = -state.imu.output.roll / 180.0f * M_PI;
			theta = state.imu.output.pitch / 180.0f * M_PI;
			sintheta = sinf(theta);
			costheta = cosf(theta);
			sinomega = sinf(omega);
			cosomega = cosf(omega);

			imuAccRotation.m11 = costheta;
			imuAccRotation.m12 = sinomega * sintheta ;
			imuAccRotation.m13 = cosomega * sintheta;
			imuAccRotation.m21 = 0.0f;
			imuAccRotation.m22 = cosomega;
			imuAccRotation.m23 = -sinomega;
			imuAccRotation.m31 = -sintheta;
			imuAccRotation.m32 = sinomega * costheta;
			imuAccRotation.m33 = cosomega * costheta;
			imuAccRotation.m41 = imuAccRotation.m42 = imuAccRotation.m43 = imuAccRotation.m44 = 0;

			matrixMultiplyVector(&imuAccRotation,&state.imu.accFilteredScaled,&state.imu.accRotated);

			logValues.accRotated.a.value = state.imu.accRotated.x;
			logValues.accRotated.b.value = state.imu.accRotated.y;
			logValues.accRotated.c.value = state.imu.accRotated.z;

			logStore(LOG_ACC_ROTATED);

			state.imu.output.zSpeed += (states.imu.accRotated.z + 1.0f);
			state.imu.output.zSpeed *= 0.95f;
			state.imu.output.zPosition += state.imu.output.zSpeed;

			logValues.zSpeed.value = state.imu.output.zSpeed;
			logValues.zPosition.value = state.imu.output.zPosition;

//			logStore(LOG_IMU_ZSPEED);
//			logStore(LOG_IMU_ZPOSITION);
//		}*/

		//STATE.sensorState. quadState.imuFix = 1;
	}
}
